
#include <string.h>
#include "crc32.h"
#include "sma_frame.h"


void smaPackDword(unsigned char *buff, unsigned long dword)
{
	int i;

	for(i=0; i<4; i++)
	{
		buff[i] = (unsigned char)(dword & 0xFF);
		dword >>= 8;
	}
}

void smaPackWord(unsigned char *buff, unsigned short word)
{
	int i;

	for(i=0; i<2; i++)
	{
		buff[i] = (unsigned char)(word & 0xFF);
		word >>= 8;
	}
}

unsigned long smaUnpackDword(const unsigned char *buff)
{
	unsigned long dword;
	int i;

	dword = 0;

	for(i=0; i<4; i++)
	{
		dword <<= 8;
		dword |= buff[3-i];
	}

	return dword;
}

unsigned short smaUnpackWord(const unsigned char *buff)
{
	unsigned short word;
	int i;

	word = 0;

	for(i=0; i<2; i++)
	{
		word <<= 8;
		word |= buff[1-i];
	}

	return word;
}

int smaPackDataFrame(unsigned long buff_size,
							unsigned char *buff,
							const SmaFrameHeader *header,
							unsigned long send_len,
							const unsigned char *send_data)
{
	unsigned long i;
	unsigned long crc32;

	if(buff_size < (21 + send_len))
	{
		/* buffer size is insufficient. */
		return -1;
	}

	buff[0] = header->src_id; /* Offset 0: Source Device ID */
	buff[1] = header->dst_id; /* Offset 1: Destination Device ID */

	smaPackDword(&buff[2], header->frame_id); /* Offset 2: Frame ID */
	smaPackDword(&buff[6], header->sent_time); /* Offset 6: Sent Time */

	buff[10] = header->cmd_type; /* Offset 10: Command Type */
	smaPackWord(&buff[11], header->cmd_code); /* Offset 11: Command Opcode */

	smaPackDword(&buff[13], send_len); /* Offset 13: Data Length */

	/* Offset 17: Data */
	for(i=0; i<send_len; i++)
	{
		buff[17+i] = send_data[i];
	}

	/* Calculate CRC32 */
	crc32 = crctablefast(buff, (17 + send_len));

	/* Offset (17 + Data Length): CRC32 */
	smaPackDword(&buff[17 + send_len], crc32);

	return 0;
}

int smaUnpackDataFrame(unsigned long buff_len,
							  const unsigned char *buff,
							  SmaFrameHeader *header,
							  unsigned long recv_size,
							  unsigned long *recv_len,
							  unsigned char *recv_data)
{
	unsigned long i;
	unsigned long crc32;
	unsigned long calc_crc32;

	if(buff_len < 21)
	{
		/* Invalid frame size. Valid frame size is at least 21 bytes. */
		return -1;
	}

	/* Last 4 bytes are CRC32 */
	crc32 = smaUnpackDword(&buff[buff_len - 4]);

	/* Calculate CRC32 */
	calc_crc32 = crctablefast(buff, (buff_len - 4));

	/* Validate CRC */
	if(crc32 != calc_crc32)
	{
		/* CRC error */
		return -1;
	}

	header->src_id = buff[0]; /* Offset 0: Source Device ID */
	header->dst_id = buff[1]; /* Offset 1: Destination Device ID */

	header->frame_id = smaUnpackDword(&buff[2]); /* Offset 2: Frame ID */
	header->sent_time = smaUnpackDword(&buff[6]); /* Offset 6: Sent Time */

	header->cmd_type = buff[10]; /* Offset 10: Command Type */
	header->cmd_code = smaUnpackWord(&buff[11]); /* Offset 11: Command Opcode */

	*recv_len = smaUnpackDword(&buff[13]); /* Offset 13: Data Length */

	/* Check Data Length value. */
	if(*recv_len != (buff_len - 21))
	{
		/* Data Length value does not match calculated value. */
		return -1;
	}

	if(buff_len > (21 + recv_size))
	{
		/* recv_data buffer size is insufficient. */
		return -1;
	}

	/* Offset 17: Data */
	for(i=0; i<(*recv_len); i++)
	{
		recv_data[i] = buff[17+i];
	}

	return 0;
}

unsigned long smaCalculateFrameSize(unsigned long data_len)
{
	return data_len + 21;
}

void smaPackJointCfg(unsigned char *buff, const SmaJointCfg *cfg)
{
	float data;

	data = (float)cfg->q1;
	smaPackDword(&buff[0], *(unsigned long *)&data); /* Offset 0: Incremental Encoder 1. */

	data = (float)cfg->q2;
	smaPackDword(&buff[4], *(unsigned long *)&data); /* Offset 4: Incremental Encoder 2. */

	data = (float)cfg->q3;
	smaPackDword(&buff[8], *(unsigned long *)&data); /* Offset 8: Incremental Encoder 3. */

	data = (float)cfg->q4;
	smaPackDword(&buff[12], *(unsigned long *)&data); /* Offset 12: Incremental Encoder 4. */

	data = (float)cfg->q5;
	smaPackDword(&buff[16], *(unsigned long *)&data); /* Offset 16: Incremental Encoder 5. */

	data = (float)cfg->q6;
	smaPackDword(&buff[20], *(unsigned long *)&data); /* Offset 20: Incremental Encoder 6. */
}

void smaUnpackJointCfg(const unsigned char *buff, SmaJointCfg *cfg)
{
	unsigned long data;

	data = smaUnpackDword(&buff[0]); /* Offset 0: Incremental Encoder 1 */
	cfg->q1 = *(float *)&data;

	data = smaUnpackDword(&buff[4]); /* Offset 4: Incremental Encoder 2 */
	cfg->q2 = *(float *)&data;

	data = smaUnpackDword(&buff[8]); /* Offset 8: Incremental Encoder 3 */
	cfg->q3 = *(float *)&data;

	data = smaUnpackDword(&buff[12]); /* Offset 12: Incremental Encoder 4 */
	cfg->q4 = *(float *)&data;

	data = smaUnpackDword(&buff[16]); /* Offset 16: Incremental Encoder 5 */
	cfg->q5 = *(float *)&data;

	data = smaUnpackDword(&buff[20]); /* Offset 20: Incremental Encoder 6 */
	cfg->q6 = *(float *)&data;
}

void smaUnpackJointData(const unsigned char *buff, double data_array[6])
{
	int i;
	unsigned long data;

	for(i=0; i<6; i++)
	{
		data = buff[i*4];
		data_array[i] = *(float *)&data;
	}
}

void smaUnpackMcpStatus(const char *buff, SmaMcpStatus *status)
{
	int i;

	status->bHomed = buff[0]; /* Offset 0: Arm homing status. */

	for(i=0; i<10; i++) /* Offset 1: State of task manager. */
	{
		status->TmStates[i] = buff[1 + i];
	}

	for(i=0; i<3; i++) /* Offset 11: State of motion controller. */
	{
		status->McStates[i] = buff[11 + i];
	}

	status->VspStatus = buff[14]; /* Offset14: State of visual servo processor. */

	for(i=0; i<6; i++) /* Offset 15: State of incremental encoders. */
	{
		status->IncrSenStates[i] = buff[15 + i];
	}

	for(i=0; i<6; i++) /* Offset 21: State of absolute encoders. */
	{
		status->AbsSenStates[i] = buff[21 + i];
	}

	status->FtSenState = buff[27]; /* Offset 27: State of force/torque sensor. */
	
	for(i=0; i<6; i++) /* Offset 28: State of motor drivers. */
	{
		status->DrvMotStates[i] = buff[28 + i];
	}

	status->SelfTestStatus = buff[34]; /* Offset 34: Self test status. */

	status->nTempInABox = buff[35]; /* Offset 35: Temperature of avionics box. */

	status->nTempAtFt = buff[36]; /* Offset 36: Temperature of force/torque sensor. */
}

void smaPackPose(unsigned char *buff, const SmaPose *pose)
{
	float data;

	data = (float)pose->x;
	smaPackDword(&buff[0], *(unsigned long *)&data); /* Offset 0: X */

	data = (float)pose->y;
	smaPackDword(&buff[4], *(unsigned long *)&data); /* Offset 4: Y */

	data = (float)pose->z;
	smaPackDword(&buff[8], *(unsigned long *)&data); /* Offset 8: Z */

	data = (float)pose->EulerZ1;
	smaPackDword(&buff[12], *(unsigned long *)&data); /* Offset 12: Euler Z1 */

	data = (float)pose->EulerY;
	smaPackDword(&buff[16], *(unsigned long *)&data); /* Offset 16: Euler Y */

	data = (float)pose->EulerZ2;
	smaPackDword(&buff[20], *(unsigned long *)&data); /* Offset 20: Euler Z2 */
}

void smaUnpackPose(const unsigned char *buff, SmaPose *pose)
{
	unsigned long data;

	data = smaUnpackDword(&buff[0]); /* Offset 0: X */
	pose->x = *(float *)&data;

	data = smaUnpackDword(&buff[4]); /* Offset 4: Y */
	pose->y = *(float *)&data;

	data = smaUnpackDword(&buff[8]); /* Offset 8: Z */
	pose->z = *(float *)&data;

	data = smaUnpackDword(&buff[12]); /* Offset 12: Euler Z1 */
	pose->EulerZ1 = *(float *)&data;

	data = smaUnpackDword(&buff[16]); /* Offset 16: Euler Y */
	pose->EulerY = *(float *)&data;

	data = smaUnpackDword(&buff[20]); /* Offset 20: Euler Z2 */
	pose->EulerZ2 = *(float *)&data;
}

void smaUnpackImage(const unsigned char *buff, SmaImage *image)
{
	image->nImageID = (short)smaUnpackWord(&buff[0]); /* Offset 0: Image ID. */

	image->width = (short)smaUnpackWord(&buff[2]); /* Offset 2: Width of image. */

	image->height = (short)smaUnpackWord(&buff[4]); /* Offset 4: Height of image. */

	memcpy(image->ImageData, &buff[6], MaxImageSize); /* Offset 6: Image data. */
}


void smaUnpackPoint2Df(const unsigned char *buff, SmaPoint2Df *point)
{
	unsigned long data;

	data = smaUnpackDword(&buff[0]); /* Offset 0: x */
	point->x = *(float *)&data;

	data = smaUnpackDword(&buff[4]); /* Offset 4: y */
	point->y = *(float *)&data;
}

unsigned long smaUnpackFeature(const unsigned char *buff, unsigned long size, SmaFeatures *feature)
{
	unsigned long len;
	int i;
	int offset;

	if(size < 5)
	{
		return 0;
	}

	feature->type = (char)buff[0]; /* Offset 0: Feature type. */

	feature->nImageId = (short)smaUnpackWord(&buff[1]); /* Offset 1: Image ID. */

	feature->nFeatures = (short)smaUnpackWord(&buff[3]); /* Offset 3: Number of features. */

	len = 5 + 10 * feature->nFeatures;

	if(size < len)
	{
		return 0;
	}

	offset = 5;

	for(i=0; i<feature->nFeatures; i++) /* Offset 5: Detected features. */
	{
		feature->DetectedFeats[i].nFeatureId = (short)smaUnpackWord(&buff[offset]); /* Sub offset 0: Feature ID. */

		smaUnpackPoint2Df(&buff[offset + 2], &feature->DetectedFeats[i].pt); /* Sub offset 2: Point. */

		offset += 10;
	}

	return len;
}

unsigned long smaUnpackMatch(const unsigned char *buff, unsigned long size, SmaMatches *match)
{
	unsigned long len;
	int i;
	int offset;

	if(size < 6)
	{
		return 0;
	}

	match->nImageId = (short)smaUnpackWord(&buff[0]); /* Offset 0: Image ID of first image. */ 

	match->nImageIdTo = (short)smaUnpackWord(&buff[2]); /* Offset 2: Image ID of second image. */ 

	match->nFeatures = (short)smaUnpackWord(&buff[4]); /* Offset 4: Number of matched features. */ 

	len = 6 + 4 * match->nFeatures;

	if(size < len)
	{
		return 0;
	}

	offset = 6;
	
	for(i=0; i<match->nFeatures; i++) /* Offset 6: Corresponding features. */
	{
		match->Matches[i].nFeatureId = (short)smaUnpackWord(&buff[offset]); /* Sub offset 0: Feature ID from first image. */

		match->Matches[i].nFeatureIdTo = (short)smaUnpackWord(&buff[offset + 2]); /* Sub offset 2: Feature ID from second image. */

		offset += 4;
	}

	return len;
}

void smaUnpackPoint3Df(const unsigned char *buff, SmaPoint3Df *point)
{
	unsigned long data;

	data = smaUnpackDword(&buff[0]); /* Offset 0: X. */
	point->x = *(float *)&data;

	data = smaUnpackDword(&buff[4]); /* Offset 4: Y. */
	point->y = *(float *)&data;

	data = smaUnpackDword(&buff[8]); /* Offset 8: Z. */
	point->z = *(float *)&data;
}

unsigned long smaUnpackTargetModel(const unsigned char *buff, unsigned long size, SmaTargetModel *model)
{
	unsigned long len;
	int i;
	int offset;

	if(size < 2)
	{
		return 0;
	}

	model->nFeatures = (short)smaUnpackWord(&buff[0]); /* Offset 0: Number of features */

	len = 2 + 14 * model->nFeatures;

	if(size < len)
	{
		return 0;
	}

	offset = 2;

	for(i=0; i<model->nFeatures; i++) /* Offset 2: Model features. */
	{
		model->ModelFeatures[i].nFeatureId = (short)smaUnpackWord(&buff[offset]); /* Sub offset 0: Feature ID. */

		smaUnpackPoint3Df(&buff[offset + 2], &model->ModelFeatures[i].position); /* Sub offset 2: Position. */

		offset += 14;
	}

	return len;
}

void smaPackPath(unsigned char *buff, const SmaPath *path)
{
	float data;
	int i;
	unsigned long offset;

	buff[0] = path->type; /* Offset 0: Path type. */

	data = (float)path->fTimeInterval;
	smaPackDword(&buff[1], *(unsigned long *)&data); /* Offset 1: Time interval. */

	smaPackWord(&buff[5], path->nPoints); /* Offset 5: Number of points. */

	offset = 7;

	/* Offset 7: Path points. */
	if(path->type == 0)
	{
		for(i=0; i < path->nPoints; i++)
		{
			smaPackPose(&buff[offset], &path->points.Xs[i]);
			
			offset += 24;
		}
	}
	else
	{
		for(i=0; i < path->nPoints; i++)
		{
			smaPackJointCfg(&buff[offset], &path->points.Qs[i]);

			offset += 24;
		}
	}
}

unsigned long smaUnpackPath(const unsigned char *buff, unsigned long size, SmaPath *path)
{
	unsigned long len;
	unsigned long data;
	int i;
	unsigned long offset;

	path->type = (char)buff[0]; /* Offset 0: Path type. */

	data = smaUnpackDword(&buff[1]); /* Offset 1: Time interval. */
	path->fTimeInterval = *(float *)&data;

	path->nPoints = (short)smaUnpackWord(&buff[5]); /* Offset 5: Number of points. */

	len = 7 + 24 * path->nPoints;

	if(size < len)
	{
		return 0;
	}

	offset = 7;

	/* Offset 7: Path points. */
	if(path->type == 0)
	{
		for(i=0; i < path->nPoints; i++)
		{
			smaUnpackPose(&buff[offset], &path->points.Xs[i]);
			
			offset += 24;
		}
	}
	else
	{
		for(i=0; i < path->nPoints; i++)
		{
			smaUnpackJointCfg(&buff[offset], &path->points.Qs[i]);

			offset += 24;
		}
	}

	return len;
}

